unit Engine.Math;

interface

{$I defines.inc}



uses
  Engine.TVec, Engine.Arrays;

const
  NaN         =  0.0 / 0.0;
  Infinity    =  1.0 / 0.0;
  NegInfinity = -1.0 / 0.0;

  ZeroConst: Single = 1E-06;

type
  TLine=class;

  TPlane=class
  private
    FEquation: PSingle;

    FN: PVec4;

    FA, FB, FC: TVec4;
    FU, FV: TVec4;
  protected
    function GetComponent(const I: Integer): Single; inline;
    procedure GetUV(var U, V: TVec4);
    procedure CalcPoints();
  public
    constructor Create();
    destructor Destroy(); override;

    procedure Initialize(const P1, P2, P3: TVec4); overload;
    procedure Initialize(const X1, Y1, Z1, X2, Y2, Z2,
      X3, Y3, Z3: Single); overload;

    function DotNormal(const P: TVec4): Single;
    function DotCoord(const P: TVec4): Single;

    function Dist(const Point: TVec4): Single; //inline;
    function GetAngle(const P: TPlane): Single; inline;

    // Calculating of plain coordinates of point T.
    // Plane is given by point A and directing vectors U and V.
    // Parameter P should be received as follows: P = T - A.
    class function GetPlainCoords(const P, U, V: TVec4): TVec2; overload;
    function GetPlainCoords(const P: TVec4): TVec2; overload;

    // Calculating intersection of the plane and line.
    // Point O belongs to the plane, X and Y are its directing vectors.
    // Line is given by point P and directing vector W.
    class function Intersect(const O, X, Y, P, W: TVec4): TVec4; overload;
    class function IntersectRange(const O, X, Y, P, W: TVec4): TVec4; overload;

    class function LineTriangleIntersect(const A, B, C, P, W: TVec4): TVec4;

    function Intersect(const L: TLine): TVec4; overload; inline;
    function Intersect(const X, W: TVec4): TVec4; overload;
    function IntersectRange(const X, W: TVec4): TVec4; overload;

    procedure Normalize();

    procedure ExtractLeftPlane(const M: PSingle);
    procedure ExtractRightPlane(const M: PSingle);

    procedure ExtractBottomPlane(const M: PSingle);
    procedure ExtractTopPlane(const M: PSingle);

    procedure ExtractNearPlane(const M: PSingle);
    procedure ExtractFarPlane(const M: PSingle);

    procedure Copy(const P: TPlane);

    property Equation: PSingle read FEquation;

    // A * x + B * y + C * z + D = 0
    property Component[const I: Integer]: Single read GetComponent;
    property A: Single index 0 read GetComponent;
    property B: Single index 1 read GetComponent;
    property C: Single index 2 read GetComponent;
    property D: Single index 3 read GetComponent;

    property N: PVec4 read FN;

    // Three points lying in the plane
    property PA: TVec4 read FA;
    property PB: TVec4 read FB;
    property PC: TVec4 read FC;

    // Plane's directing vectors
    property U: TVec4 read FU;
    property V: TVec4 read FV;
  end;
                    
  TLine=class
  private
    V, P: TVec4;
    VLen: Single;
  protected    
  public
    constructor Create();
    procedure Initialize(const P1, P2: TVec4);
    function GetProjCoord(const D: TVec4): Single;
    function Dist(const D: TVec4): Single;

    // Distance between line (P1-P2) and point P3
    class function LineDist(const P1, P2, P3: TVec4): Single;

    // Intersection of lines which are given by point and directing vector:
    // P01 lies in the first line, and U is its vector;
    // P02 lies in the second line, and V is its vector.
    class function LineIntersect(const P01, P02, U, V: TVec4): TVec4;
  end;

  ConvexHull2D = class abstract
  public
    class function GiftWrap(const Coords: array of TVec2;
      const CoordsCount: Integer): ArrayOfInt;
    class function QuickHull(const Coords: array of TVec2;
      const CoordsCount: Integer): ArrayOfInt;
  end;

  function EqualToZero(const Value: Single): Boolean; inline;

  function Tan(const X: Extended): Extended;
  function CoTan(const X: Extended): Extended;
  function ArcCos(const X : Single) : Single;
  function ArcSin(const X : Single) : Single;

  function Mini(const A, B: Integer): Integer; inline;
  function Minf(const A, B: Single): Single; inline;

  function Maxf(const A, B: Single): Single; inline;
  function Maxi(const A, B: Integer): Integer; inline;

  function Clampi(const V: Integer; const Min, Max: Integer): Integer;
  function Clampf(const V: Single; const Min, Max: Single): Single;

  procedure Swapi(var A, B: Integer); inline;
  procedure Swapf(var A, B: Single); inline;

  procedure Frexp(const X: Extended; var Mantissa: Extended; var Exponent: Integer);

  function Powf(const X, Power: Single): Single;

  function Sign(const S: Single): Integer; inline;

  function RcpSqrt(const X: Single): Single;
  function ATanPositive(const Y, X: Single): Single;

implementation

{$POINTERMATH ON}

const
  SinC0: Single = -2.39e-08;
  SinC1: Single =  2.7526e-06;
  SinC2: Single = -1.98409e-04;
  SinC3: Single = 8.3333315e-03;
  SinC4: Single = -1.666666664e-01;
  SinC5: Single = 1.0;

  Pi2: Single = Pi/2;
  ATanC1: Single = 0.0028662257;
  ATanC2: Single = -0.0161657367;
  ATanC3: Single = 0.0429096138;
  ATanC4: Single = -0.0752896400;
  ATanC5: Single = 0.1065626393;
  ATanC6: Single = -0.1420889944;
  ATanC7: Single = 0.1999355085;
  ATanC8: Single = -0.3333314528;

  C05: Single = 0.5;
  C15: Single = 1.5;

function RcpSqrt(const X: Single): Single;
//var
//  I: LongInt;
//begin
//  I:=Integer((@X)^);
//  I:=$5F3759DF - (I shr 1);
//  Result:=Single((@I)^);
//  Result:=Result * (1.5 - Result * Result * X * 0.5);
asm
  FLD X
  FMUL C05
  MOV EAX, [X]
  SHR EAX, 1
  SUB EAX, $5F3759DF
  NEG EAX
  PUSH EAX
  FMUL [ESP]
  FMUL [ESP]
  FLD C15
  FSUBR
  FMUL [ESP]
  POP EAX
end;

function SinZeroHalfPi(const A: Single): Single;
var
  SinTemp: Single;
asm
  PUSH EBX
  PUSH ESI
  PUSH EDI

  FLD A
  FMUL ST(0), ST(0);
  FST SinTemp
  FMUL SinC0
  FADD SinC1
  FMUL SinTemp
  FADD SinC2
  FMUL SinTemp
  FADD SinC3
  FMUL SinTemp
  FADD SinC4
  FMUL SinTemp
  FADD SinC5
  FMUL A

  POP EDI
  POP ESI
  POP EBX
end;

function ATanPositive(const Y, X: Single): Single;
var
  A, D, S, T: Single;
asm
  FLD Y
  FLD X
  FCOM
  FSTSW
  SAHF
  JNB @Below
  FLD Pi2
  FSTP D

  FCHS
  FDIVR
  FST A
  JMP @EndIf
  @Below:
  FLDZ
  FSTP D

  FDIV
  FST A
  @EndIf:

  FLD ST(0)
  FMUL ST(0), ST(0)
  FST S

  FMUL ATanC1
  FADD ATanC2
  FMUL S

  FADD ATanC3
  FMUL S

  FADD ATanC4
  FMUL S

  FADD ATanC5
  FMUL S

  FADD ATanC6
  FMUL S

  FADD ATanC7
  FMUL S

  FADD ATanC8
  FMUL S

  FLD1
  FADDP
  FMULP
  FADD D
end;

function Sign(const S: Single): Integer; inline;
begin
  if S>ZeroConst then
    Result:=1
  else
//  if S<ZeroConst then
    Result:=-1
//  else
//    Result:=0
end;

function ArcCos(const X : Single) : Single; overload;
asm
  //Result := ArcTan2(Sqrt((1+X) * (1-X)), X)
  FLD1
  FLD    X
  FST    ST(2)
  FMUL   ST(0), ST(0)
  FSUBP
  FSQRT
  FXCH
  FPATAN
end;

function ArcSin(const X : Single) : Single;
asm
  //Result := ArcTan2(X, Sqrt((1+X) * (1-X)))
  fld1
  fld    X
  fst    st(2)
  fmul   st(0), st(0)
  fsubp
  fsqrt
  fpatan
end;

procedure Frexp(const X: Extended; var Mantissa: Extended; var Exponent: Integer);
{ Mantissa ptr in EAX, Exponent ptr in EDX }
asm
  FLD     X
  PUSH    EAX
  MOV     dword ptr [edx], 0    { if X = 0, return 0 }

  FTST
  FSTSW   AX
  FWAIT
  SAHF
  JZ      @@Done

  FXTRACT                 // ST(1) = exponent, (pushed) ST = fraction
  FXCH

// The FXTRACT instruction normalizes the fraction 1 bit higher than
// wanted for the definition of frexp() so we need to tweak the result
// by scaling the fraction down and incrementing the exponent.

  FISTP   dword ptr [edx]
  FLD1
  FCHS
  FXCH
  FSCALE                  // scale fraction
  INC     dword ptr [edx] // exponent biased to match
  FSTP ST(1)              // discard -1, leave fraction as TOS

@@Done:
  POP     EAX
  FSTP    tbyte ptr [eax]
  FWAIT
end;

function Powf(const X, Power: Single): Single;
begin
  Result:=Exp(Power * Ln(X));
end;

function Tan(const X: Extended): Extended;
{  Tan := Sin(X) / Cos(X) }
asm
  FLD    X
  FPTAN
  FSTP   ST(0)      { FPTAN pushes 1.0 after result }
  FWAIT
end;

function CoTan(const X: Extended): Extended;
asm
  FLD X
  FPTAN
  FDIVRP
  FWAIT
end;

procedure TLine.Initialize;
begin
  V.SetValuef(P2.X-P1.X, P2.Y-P1.Y, P2.Z-P1.Z, 1);
  P.SetValuev(P1);
  VLen:=V.Len();
end;

function TPlane.Intersect(const L: TLine): TVec4;
begin
  Result:=Intersect(L.P, L.V);
end;

function TPlane.GetAngle;
begin
  Result:=A*P.A+B*P.B+C*P.C;
end;

function TPlane.GetComponent(const I: Integer): Single;
begin
  Result:=FEquation[I];
end;

function TPlane.GetPlainCoords(const P: TVec4): TVec2;
begin
  Result:=GetPlainCoords(P, U, V)
end;

procedure TPlane.GetUV(var U, V: TVec4);
begin
  if EqualToZero(A) then
    if EqualToZero(B) then
      if EqualToZero(C) then
      begin
        U.SetValuef(0, 0, 0, 1);
        V.SetValuef(0, 0, 0, 1)
      end
      else
      begin
        U.SetValuef(1, 0, -A/C, 1);
        V.SetValuef(0, 1, -B/C, 1)
      end
    else
    begin
      U.SetValuef(1, -A/B, 0, 1);
      V.SetValuef(0, -C/B, 1, 1)
    end
  else
  begin
    U.SetValuef(-B/A, 1, 0, 1);
    V.SetValuef(-C/A, 0, 1, 1)    
  end;
  U.Normalize();
  V.Normalize();
end;

function TPlane.Intersect(const X, W: TVec4): TVec4;
var
  DD: Single;
  VV: TVec4;
  E: Single;
begin
  VV:=PA-X;
  DD:=A*VV.X+B*VV.Y+C*VV.Z;
  E:=A*W.X+B*W.Y+C*W.Z;
  if EqualToZero(E) then
    Result.SetValuef(0, 0, 0, 0)
  else
    Result.SetValuev(X+W*(DD/E));
end;

function TPlane.IntersectRange(const X, W: TVec4): TVec4;
var
  DD: Single;
  VV: TVec4;
  E: Single;
begin
  VV:=PA-X;
  DD:=A*VV.X+B*VV.Y+C*VV.Z;
  E:=A*W.X+B*W.Y+C*W.Z;
  if EqualToZero(E) then
    Result.SetValuef(0, 0, 0, 0)
  else
    if (DD / E > 0.0) and (DD / E < 1.0) then
      Result.SetValuev(X+W * (DD/E))
    else
    Result.SetValuef(0, 0, 0, 0)
end;

class function TPlane.IntersectRange(const O, X, Y, P, W: TVec4): TVec4;
var
  DP: Single;
  V: TVec4;
  E: Single;
  N: TVec4;
begin
  V:=O - P;
  if EqualToZero(V.SquareLen()) then
    V.SetValuev(X);

  N:=X.CrossProduct(Y);

  DP:=N.DotProduct(V);
  E:=N.DotProduct(W);

  if EqualToZero(E) then
    Result.SetValuef(0.0, 0.0, 0.0, 0.0)
  else
  begin
    DP:=DP*(1/E);
    if (DP >= 0.0) and (DP <= 1.0) then
      Result.SetValuev(P+W*DP*(1/E))
    else
      Result.SetValuef(0.0, 0.0, 0.0, 0.0)
  end;
end;

class function TPlane.LineTriangleIntersect(const A, B, C, P,
  W: TVec4): TVec4;
var
  Pa1: TVec4;
  Pa2: TVec4;
  Pa3: TVec4;

  A1, A2, A3: Single;
begin
  Result:=Intersect(A, B-A, C-A, P, W);

  if Result.W=0.0 then
    Exit;

  Pa1:=A-Result;
  Pa2:=B-Result;
  Pa3:=C-Result;
  Pa1.Normalize();
  Pa2.Normalize();
  Pa3.Normalize();

  A1:=ArcCos(Pa1.DotProduct(Pa2));
  A2:=ArcCos(Pa2.DotProduct(Pa3));
  A3:=ArcCos(Pa3.DotProduct(Pa1));

  if not (Abs(A1+A2+A3-2*Pi)<0.001) then
    Result.W:=0.0;
end;

destructor TPlane.Destroy;
begin
  FreeMem(FEquation);
  inherited;
end;

function TPlane.Dist;
begin
  Result:=A*Point.X+B*Point.Y+C*Point.Z+D;
end;

function TPlane.DotCoord(const P: TVec4): Single;
begin
  Result:=A * P.X + B * P.Y + C * P.Z + D;
end;

function TPlane.DotNormal(const P: TVec4): Single;
begin
  Result:=A * P.X + B * P.Y + C * P.Z;
end;

procedure TPlane.ExtractBottomPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] + M[1];
  FEquation[1]:=M[7] + M[5];
  FEquation[2]:=M[11] + M[9];
  FEquation[3]:=M[15] + M[13];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.ExtractFarPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] - M[2];
  FEquation[1]:=M[7] - M[6];
  FEquation[2]:=M[11] - M[10];
  FEquation[3]:=M[15] - M[14];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.ExtractLeftPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] + M[0];
  FEquation[1]:=M[7] + M[4];
  FEquation[2]:=M[11] + M[8];
  FEquation[3]:=M[15] + M[12];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.ExtractNearPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] + M[2];
  FEquation[1]:=M[7] + M[6];
  FEquation[2]:=M[11] + M[10];
  FEquation[3]:=M[15] + M[14];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.ExtractRightPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] - M[0];
  FEquation[1]:=M[7] - M[4];
  FEquation[2]:=M[11] - M[8];
  FEquation[3]:=M[15] - M[12];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.ExtractTopPlane(const M: PSingle);
begin
  FEquation[0]:=M[3] - M[1];
  FEquation[1]:=M[7] - M[5];
  FEquation[2]:=M[11] - M[9];
  FEquation[3]:=M[15] - M[13];
  Normalize();
  GetUV(FU, FV);
  CalcPoints();
end;

procedure TPlane.Initialize(const P1, P2, P3: TVec4);
var
  V3: TVec4;
begin
  FU:=P2-P1;
  FV:=P3-P1;
  V3:=FU.CrossProduct(FV);
  V3.Normalize();

  FEquation[0]:=V3.X;
  FEquation[1]:=V3.Y;
  FEquation[2]:=V3.Z;
  FEquation[3]:=-V3.DotProduct(P1);

  FA:=P1;
  FB:=P2;
  FC:=P3;
end;

procedure TPlane.Initialize(const X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3: Single);
var
  T, V3: TVec4;
  L: Single;
begin
  FU.SetValuef(X2-X1, Y2-Y1, Z2-Z1, 1);
  FV.SetValuef(X3-X1, Y3-Y1, Z3-Z1, 1);

  V3:=FU.CrossProduct(FV);

  V3.Normalize();

  T.X:=X1;
  T.Y:=Y1;
  T.Z:=Z1;
  L:=V3.DotProduct(T);

  FEquation[0]:=V3.X;
  FEquation[1]:=V3.Y;
  FEquation[2]:=V3.Z;
  FEquation[3]:=-L;

  FA.SetValuef(X1, Y1, Z1, 1);
  FB.SetValuef(X2, Y2, Z2, 1);
  FC.SetValuef(X3, Y3, Z3, 1);
end;

procedure TPlane.CalcPoints;
begin
  if Abs(C)>0.1 then
  begin
    FA.SetValuef(0, 0, -D/C, 1.0);
    FB.SetValuev(FA+FU);
    FC.SetValuev(FA+FV);
  end
  else
  if Abs(B)>0.1 then
  begin
    FA.SetValuef(0, -D/B, 0, 1.0);
    FB.SetValuev(FA+FU);
    FC.SetValuev(FA+FV);
  end
  else
  if Abs(A)>0.1 then
  begin
    FA.SetValuef(-D/A, 0, 0, 1.0);
    FB.SetValuev(FA+FU);
    FC.SetValuev(FA+FV);
  end
end;

procedure TPlane.Copy(const P: TPlane);
begin
  FEquation[0]:=P.FEquation[0];
  FEquation[1]:=P.FEquation[1];
  FEquation[2]:=P.FEquation[2];
  FEquation[3]:=P.FEquation[3];

  FA.SetValuev(P.FA);
  FB.SetValuev(P.FB);
  FC.SetValuev(P.FC);

  FU.SetValuev(P.U);
  FV.SetValuev(P.V);
end;

constructor TPlane.Create;
begin
  FEquation:=AllocMem(SizeOf(Single) * 4);
  FN:=Pointer(FEquation);
end;

{ TMath }

function Maxf(const A, B: Single): Single;
begin
  if A>B then
    Result:=A
  else
    Result:=B;
end;

class function ConvexHull2D.GiftWrap(const Coords: array of TVec2;
  const CoordsCount: Integer): ArrayOfInt;
var
  Count, MaxI, I, J: Integer;
  T, L: Single;
  B: Boolean;
  Vec, NewVec: TVec2;
  Mark: array of Boolean;
begin
  SetLength(Result, CoordsCount);
  SetLength(Mark, CoordsCount);

  try
    for I := 0 to CoordsCount - 1 do
      Mark[I]:=False;

    MaxI:=0;
    T:=Coords[0].X;

    for I := 0 to CoordsCount - 1 do
      if Coords[I].X>T then
      begin
        T:=Coords[I].X;
        MaxI:=I;
      end;

    Count:=0;
    while True do
    begin
      Result[Count]:=MaxI;
      Inc(Count);
      Mark[MaxI]:=True;

      MaxI:=-1;

      for J := 0 to CoordsCount - 1 do
      begin
        if J=Result[Count-1] then
          Continue;

        MaxI:=J;
        Vec:=Coords[J]-Coords[Result[Count-1]];
        if Vec.IsZero() then
          Continue;

        B:=False;
        for I := 0 to CoordsCount - 1 do
        begin
          if MaxI=I then
            Continue;

          NewVec:=Coords[I]-Coords[Result[Count-1]];
          if NewVec.IsZero() then
            Continue;
        
          L:=NewVec.CrossProductLength(Vec);
          if (L>0)or(EqualToZero(L)and(NewVec.SquareLen()>Vec.SquareLen())) then
          begin
            B:=True;
            Break;
          end;
        end;

        if not B then
          Break;
      end;
    
      if Mark[MaxI] then
        Break;
    end;
    SetLength(Result, Count);
  finally
    SetLength(Mark, 0);
  end;
end;

function EqualToZero(const Value: Single): Boolean;
begin
  Result:=Abs(Value)<ZeroConst;
end;

class function TPlane.Intersect(const O, X, Y, P, W: TVec4): TVec4;
var
  DP: Single;
  V: TVec4;
  E: Single;
  N: TVec4;
begin
  V:=O - P;
  if EqualToZero(V.SquareLen()) then
    V.SetValuev(X);

  N:=X.CrossProduct(Y);

  DP:=N.DotProduct(V);
  E:=N.DotProduct(W);

  if EqualToZero(E) then
    Result.SetValuef(0.0, 0.0, 0.0, 0.0)
  else
    Result.SetValuev(P+W*DP*(1/E));
end;

class function TLine.LineDist(const P1, P2, P3: TVec4): Single;
var
  U, W: TVec4;
begin
  U:=P1-P2;
  U.Normalize();
  W:=P3-P2;
  Result:=U.CrossProduct(W).Len();
end;

class function TLine.LineIntersect(const P01, P02, U, V: TVec4): TVec4;
var
  Beta: Single;
  D: Single;
begin
  D:=(U.Y * V.X - V.Y * U.X);
  Assert(Abs(D) > ZeroConst);
  Beta:=(U.Y * (P01.X - P02.X) + U.X * (P02.Y - P01.Y)) / D;
  Result:=P02 + V * Beta;
end;

function Maxi(const A, B: Integer): Integer;
begin
  if A>B then
    Result:=A
  else
    Result:=B;
end;

function Mini(const A, B: Integer): Integer;
begin
  if A>B then
    Result:=B
  else
    Result:=A;
end;

function Minf(const A, B: Single): Single;
begin
  if A>B then
    Result:=B
  else
    Result:=A;
end;

function Clampi(const V: Integer; const Min, Max: Integer): Integer;
begin
  if V > Max then
    Result:=Max
  else
  if V < Min then
    Result:=Min
  else
    Result:=V;
end;

function Clampf(const V: Single; const Min, Max: Single): Single;
begin
  if V > Max then
    Result:=Max
  else
  if V < Min then
    Result:=Min
  else
    Result:=V;
end;

class function TPlane.GetPlainCoords(const P, U, V: TVec4): TVec2;
var
  T: Single;
begin
  Result.SetValue(0, 0);
  if not EqualToZero(U.X) then
  begin
    T:=V.Y*U.X-V.X*U.Y;
    if not EqualToZero(T) then
    begin
      Result.Y:=(P.Y*U.X-P.X*U.Y)/T;
      Result.X:=(P.X-Result.Y*V.X)/U.X;
    end
    else
    begin
      T:=V.Z*U.X-V.X*U.Z;
      if not EqualToZero(T) then
      begin
        Result.Y:=(P.Z*U.X-P.X*U.Z)/T;
        Result.X:=(P.X-Result.Y*V.X)/U.X;
      end
      else
        Exit;
    end;
  end
  else
  if not EqualToZero(U.Y) then
  begin
    T:=V.Z*U.Y-V.Y*U.Z;
    if not EqualToZero(T) then
    begin
      Result.Y:=(P.Z*U.Y-P.Y*U.Z)/T;
      Result.X:=(P.Y-Result.Y*V.Y)/U.Y;
    end
    else
    begin
      T:=V.X*U.Y-V.Y*U.X;
      if not EqualToZero(T) then
      begin
        Result.Y:=(P.X*U.Y-P.Y*U.X)/T;
        Result.X:=(P.Y-Result.Y*V.Y)/U.Y;
      end
      else
        Exit;
    end
  end
  else
  if not EqualToZero(U.Z) then
  begin
    T:=V.X*U.Z-V.Z*U.X;
    if not EqualToZero(T) then
    begin
      Result.Y:=(P.X*U.Z-P.Z*U.X)/T;
      Result.X:=(P.Z-Result.Y*V.Z)/U.Z;
    end
    else
    begin
      T:=V.Y*U.Z-V.Z*U.Y;
      if not EqualToZero(T) then
      begin
        Result.Y:=(P.Y*U.Z-P.Z*U.Y)/T;
        Result.X:=(P.Z-Result.Y*V.Z)/U.Z;
      end;
    end;
  end
  else
    Exit;
end;

procedure Swapi(var A, B: Integer);
var
  C: Integer;
begin
  C:=A;
  A:=B;
  B:=C;
end;

procedure Swapf(var A, B: Single);
var
  T: Single;
begin
  T:=A;
  A:=B;
  B:=T;
end;

{TLine}

constructor TLine.Create;
begin
  inherited;
end;

function TLine.Dist(const D: TVec4): Single;
begin
  Result:=V.CrossProduct(D-P).Len()/VLen;
end;

function TLine.GetProjCoord(const D: TVec4): Single;
begin
  Result:=V.DotProduct(D-P)/VLen;
end;

procedure TPlane.Normalize;
var
  InvL: Single;
begin
  InvL:=Sqrt(Sqr(A)+Sqr(B)+Sqr(C));
  if InvL <= ZeroConst then
    Exit;

  InvL:=1.0 / InvL;// * Sign(FEquation[3]);
  FEquation[0]:=FEquation[0] * InvL;
  FEquation[1]:=FEquation[1] * InvL;
  FEquation[2]:=FEquation[2] * InvL;
  FEquation[3]:=FEquation[3] * InvL;
end;

class function ConvexHull2D.QuickHull(const Coords: array of TVec2;
  const CoordsCount: Integer): ArrayOfInt;

  function Rekk(const A, B, C: Integer; S: ArrayOfInt): ArrayOfInt;
  var
    VA, VB, VC: TVec2;
    AC: TVec2;
    I: Integer;
    LACS: Integer;
    ACS: ArrayOfInt;
    CP: Single;
    ACCL: Single;
    ACC: Integer;
    ACLen: Single;

    ACConvex, CBConvex: ArrayOfInt;
    CB: TVec2;
  begin
    Assert(Length(S)>0);

    VA:=Coords[A];
    VB:=Coords[B];
    VC:=Coords[C];

    AC:=VC-VA;
    CB:=VB-VC;
    ACLen:=Sqrt(AC.SquareLen());

    SetLength(ACS, 10);

    LACS:=0;
    ACCL:=-1;
    ACC:=-1;

    for I := 0 to Length(S) - 1 do
    begin
      if S[I]=C then
        Continue;
      CP:=AC.CrossProductLength(Coords[S[I]]-VA);
      if CP>0 then
      begin
        if LACS>=Length(ACS) then
          SetLength(ACS, 2*LACS);
        ACS[LACS]:=S[I];
        Inc(LACS);

        if ACCL<CP/ACLen then
        begin
          ACC:=S[I];
          ACCL:=CP/ACLen;
        end;
      end;
    end;

    SetLength(ACS, LACS);

    if Length(ACS)>0 then
      ACConvex:=Rekk(A, C, ACC, ACS)
    else
      ACConvex:=nil;

    //=================

    LACS:=0;
    ACCL:=-1;
    ACC:=-1;
    SetLength(ACS, 10);

    for I := 0 to Length(S) - 1 do
    begin
      if S[I]=C then
        Continue;
      CP:=CB.CrossProductLength(Coords[S[I]]-VC);
      if CP>0 then
      begin
        if LACS>=Length(ACS) then
          SetLength(ACS, 2*LACS);
        ACS[LACS]:=S[I];
        Inc(LACS);

        if ACCL<CP/ACLen then
        begin
          ACC:=S[I];
          ACCL:=CP/ACLen;
        end;
      end;
    end;

    SetLength(ACS, LACS);

    if Length(ACS)>0 then
      CBConvex:=Rekk(C, B, ACC, ACS)
    else
      CBConvex:=nil;


    SetLength(Result, Length(ACConvex)+Length(CBConvex)+1);

    for I := 0 to Length(ACConvex) - 1 do
      Result[I]:=ACConvex[I];
    Result[Length(ACConvex)]:=C;
    for I := 0 to Length(CBConvex) - 1 do
      Result[Length(ACConvex)+1+I]:=CBConvex[I];
  end;

var
  A, B, ABC, BAC: Integer;

  BX, AX: Single;
  ABS, BAS: ArrayOfInt;
  I: Integer;

  AB: TVec2;
  CP: Single;
  ABLen: Single;
  ABCLen, BACLen: Single;
  ABSCount, BASCount: Integer;
  ABConvex, BAConvex: ArrayOfInt;
begin
  BX:=-1;
  AX:=Coords[0].X;
  A:=0;
  B:=1;

  ABSCount:=0;
  BASCount:=0;
  SetLength(ABS, CoordsCount shr 1);
  SetLength(BAS, CoordsCount shr 1);

  for I := 0 to CoordsCount - 1 do
  begin
    if Coords[I].X>BX then
    begin
      BX:=Coords[I].X;
      B:=I;
    end;
    if Coords[I].X<AX then
    begin
      AX:=Coords[I].X;
      A:=I;
    end;
  end;

  AB:=Coords[B]-Coords[A];
  ABLen:=Sqrt(AB.SquareLen());
  ABCLen:=-1;

  BACLen:=-1;
  ABC:=-1;
  BAC:=-1;

  for I := 0 to CoordsCount - 1 do
  begin
    if (I<>A)and(I<>B) then
    begin
      CP:=AB.CrossProductLength(Coords[I]-Coords[A]);
      if (CP>0) then
      begin
        if ABSCount>=Length(ABS) then
          SetLength(ABS, 2*ABSCount);
        ABS[ABSCount]:=I;
        Inc(ABSCount);
        if (CP/ABLen>ABCLen) then
        begin
          ABCLen:=CP/ABLen;
          ABC:=I;
        end;
      end
      else
      begin
        if BASCount>=Length(BAS) then
          SetLength(BAS, 2*BASCount);
        BAS[BASCount]:=I;
        Inc(BASCount);
        if (-CP/ABLen>BACLen) then
        begin
          BACLen:=-CP/ABLen;
          BAC:=I;
        end;
      end
    end;
  end;

  SetLength(ABS, ABSCount);
  SetLength(BAS, BASCount);

  if ABCLen>=0.0 then
    ABConvex:=Rekk(A, B, ABC, ABS);

  if BACLen>=0.0 then
    BAConvex:=Rekk(B, A, BAC, BAS);

  SetLength(Result, Length(ABConvex)+Length(BAConvex)+2);
  Result[0]:=A;
  Move(ABConvex[0], Result[1], SizeOf(Result[0])*Length(ABConvex));
  Result[Length(ABConvex)+1]:=B;
  Move(BAConvex[0], Result[Length(ABConvex)+2], SizeOf(Result[0])*Length(BAConvex));
end;

end.

